#include "WxPointedSticksInteractor.h"
#include <vtkPlane.h>
#include <vtkCamera.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkInteractorObserver.h>
#include <vtkTransform.h>
#include <itkEuler3DTransform.h>
#include <mitkLine.h>
#include <mitkNodePredicateDataType.h>
#include <mitkEvent.h>
#include <mitkDisplayPositionEvent.h>
#include <mitkDisplayGeometry.h>
#include <mitkGeometry2DData.h>
#include <mitkRenderingManager.h>
#include <mitkUndoController.h>
#include <mitkStateEvent.h>
#include <mitkState.h>
#include <mitkBaseRenderer.h>
#include <mitkInteractionConst.h>
#include <mitkAction.h>
#include <mitkProperties.h>


using namespace mitk;
#define LARGE_DOUBLE 9999999;
#define ROUTE_LENGTH 250

WxPointedSticksInteractor::WxPointedSticksInteractor(const char * type,DataNode* normalRouteModel):StateMachine(type)
{
    m_normalRouteModel=normalRouteModel;
    m_normalRouteModelSurface=dynamic_cast<Surface*>(m_normalRouteModel->GetData());
    m_ViewPlaneGeometry=PlaneGeometry::New();

    normalRouteModel->AddProperty("SelectedColor",mitk::ColorProperty::New(1,0,0));
    normalRouteModel->AddProperty("DeSelectedColor",mitk::ColorProperty::New(0.5,0,0));
    normalRouteModel->SetColor(0.5,0,0);
}

WxPointedSticksInteractor::~WxPointedSticksInteractor(void)
{

}

bool WxPointedSticksInteractor::ExecuteAction( mitk::Action* action, mitk::StateEvent const* stateEvent )
{
    bool ok=false;
    const mitk::Event *theEvent = stateEvent->GetEvent();
    mitk::ScalarType timeInMS = 0.0;
    if ( theEvent )
    {
        if (theEvent->GetSender() != NULL)
        {
            timeInMS = theEvent->GetSender()->GetTime();
        }
    }
    const DisplayPositionEvent* posEvent = dynamic_cast<const DisplayPositionEvent*>(stateEvent->GetEvent());
    if (!posEvent) return false;
    bool f;
    m_normalRouteModel->GetVisibility(f,theEvent->GetSender());
    if(!f) return false;

    switch (action->GetActionId())
    {

    case AcCHECKPOINT:
        {  const Geometry3D* geometry=m_normalRouteModelSurface->GetGeometry();
            mitk::Vector3D needleDirection=geometry->GetAxisVector(0);
            mitk::Vector3D needleWide=geometry->GetAxisVector(1);//与2Dmapper对应
            mitk::Point3D needleOrigin=geometry->GetOrigin();

            mitk::Point3D needleEnd=needleOrigin+needleDirection;
            mitk::Point2D needleOrigin2d,needleEnd2d,
            cursor2d=posEvent->GetDisplayPosition();

            if(posEvent->GetSender()->GetMapperID()==mitk::BaseRenderer::Standard3D)
            {
                double vtkneedleEnd2d[3],vtkneedleOrigin2d[3];
                vtkRenderer* renderer=posEvent->GetSender()->GetVtkRenderer();
                renderer->SetWorldPoint(needleOrigin[0],needleOrigin[1],needleOrigin[2],1.0);
                renderer->WorldToDisplay();
                renderer->GetDisplayPoint(vtkneedleOrigin2d);
                renderer->SetWorldPoint(needleEnd[0],needleEnd[1],needleEnd[2],1.0);
                renderer->WorldToDisplay();
                renderer->GetDisplayPoint(vtkneedleEnd2d);
                needleOrigin2d[0]=vtkneedleOrigin2d[0];
                needleOrigin2d[1]=vtkneedleOrigin2d[1];
                needleEnd2d[0]=vtkneedleEnd2d[0];
                needleEnd2d[1]=vtkneedleEnd2d[1];
            }
            else
            {
                mitk::DisplayGeometry::Pointer dg=posEvent->GetSender()->GetDisplayGeometry();
                dg->Map(needleEnd,needleEnd2d);
                dg->Map(needleOrigin,needleOrigin2d);
                dg->WorldToDisplay(needleOrigin2d, needleOrigin2d);
                dg->WorldToDisplay(needleEnd2d, needleEnd2d);
            }
            Line<ScalarType,2> *needle = new Line<ScalarType,2>();
            needle->SetPoints(needleOrigin2d, needleEnd2d);
            double thisDistance = needle->Distance(cursor2d);
            double disToEnd=(needleEnd2d-needle->Project(cursor2d)).GetNorm();
            double disToOri=(needleOrigin2d-needle->Project(cursor2d)).GetNorm();
            delete needle;
            StateEvent *newStateEvent(NULL);
            if (thisDistance<needleWide.GetNorm()&&disToEnd<=needle->GetDirection().GetNorm()&&disToOri<=needle->GetDirection().GetNorm())
                newStateEvent = new StateEvent(EIDYES, stateEvent->GetEvent());
            else
                newStateEvent = new StateEvent(EIDNO, stateEvent->GetEvent());
            this->HandleEvent( newStateEvent );
            delete newStateEvent;
        ok = true;
        break;
    }

    case AcMODESELECT:
            {
                ColorProperty::Pointer colorProperty;
                colorProperty = dynamic_cast<ColorProperty*>(m_normalRouteModel->GetProperty( "SelectedColor" ));
                if ( colorProperty.IsNotNull() )
                {
                    const Color& color = colorProperty->GetColor();
                    m_normalRouteModel->SetColor(color);
                }
                ok = true;
                break;
            }
    case AcSELECT:
            {
                Superclass::ExecuteAction( action, stateEvent );
                if(posEvent->GetSender()->GetMapperID()==mitk::BaseRenderer::Standard3D)
                {
                    mitk::BaseRenderer *renderer = NULL;
                    vtkRenderWindow *renderWindow = NULL;
                    vtkRenderWindowInteractor *renderWindowInteractor = NULL;

                    renderer = posEvent->GetSender();
                    if ( renderer != NULL )
                    {
                      renderWindow = renderer->GetRenderWindow();
                      if ( renderWindow != NULL )
                      {
                        renderWindowInteractor = renderWindow->GetInteractor();
                      }
                    }
                    renderWindowInteractor->Disable();
                    Point3D cursor = posEvent->GetWorldPosition();
                    Vector3D v3dViewPlaneNormal;
                    double *ViewPlaneNormal= posEvent->GetSender()->GetVtkRenderer()->GetActiveCamera()->GetViewPlaneNormal();
                    v3dViewPlaneNormal[0]=ViewPlaneNormal[0];
                    v3dViewPlaneNormal[1]=ViewPlaneNormal[1];
                    v3dViewPlaneNormal[2]=ViewPlaneNormal[2];
                    m_OriginOld=m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin();
                    m_ViewPlaneGeometry->InitializePlane(m_OriginOld,v3dViewPlaneNormal);

                    m_v3dModelDirection=m_normalRouteModelSurface->GetGeometry()->GetAxisVector(0);
                    m_CursorOld2d=posEvent->GetDisplayPosition();

                    Point2D p2d1,p2d2;
                    m_ViewPlaneGeometry->Map(cursor,p2d1);
                    m_ViewPlaneGeometry->Map(m_OriginOld,p2d2);
                    m_v2dOnViewPlane=p2d1-p2d2;

                    mitk::Vector3D v1=cursor-m_OriginOld;
                    m_VecLength3d=v1.GetNorm();
                }
                else
                {
                    posEvent->GetSender()->GetDisplayGeometry();
                    Point3D cursor = posEvent->GetWorldPosition();
                    m_OriginOld=m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin();

                    m_v3dModelDirection=m_normalRouteModelSurface->GetGeometry()->GetAxisVector(0);
                    m_CursorOld2d=posEvent->GetDisplayPosition();

                    Point2D p2d1,p2dOri,p2dEnd;
                    mitk::DisplayGeometry::Pointer dg=posEvent->GetSender()->GetDisplayGeometry();
                    dg->Map(cursor,p2d1);
                    dg->Map(m_OriginOld,p2dOri);
                    m_v2dOnViewPlane=p2d1-p2dOri;

                    mitk::Point3D needleEnd=m_OriginOld+m_v3dModelDirection;
                    dg->Map(needleEnd,p2dEnd);
                    Vector2D needle2d=p2dEnd-p2dOri;

                    cursor=m_OriginOld+m_v3dModelDirection*(m_v2dOnViewPlane.GetNorm()/needle2d.GetNorm());

                    mitk::Vector3D v1=cursor-m_OriginOld;
                    m_VecLength3d=v1.GetNorm();
                }
                ok = true;
                break;
            }
    case AcMOVEPOINT:   //平移
            {
                Point3D cursor = posEvent->GetWorldPosition();
                Point2D p2d1,p2d2;
                if(posEvent->GetSender()->GetMapperID()==mitk::BaseRenderer::Standard3D)
                        m_ViewPlaneGeometry->Map(cursor,p2d1);
                else
                        posEvent->GetSender()->GetDisplayGeometry()->Map(cursor,p2d1);

                p2d2=p2d1-m_v2dOnViewPlane;
                Point3D p3d2;
                if(posEvent->GetSender()->GetMapperID()==mitk::BaseRenderer::Standard3D)
                        m_ViewPlaneGeometry->Map(p2d2,p3d2);
                else
                        posEvent->GetSender()->GetDisplayGeometry()->Map(p2d2,p3d2);

                mitk::AffineTransform3D::Pointer affineTransform = m_normalRouteModelSurface->GetGeometry()->GetIndexToWorldTransform();
                mitk::Vector3D pos;
                pos.Set_vnl_vector(p3d2.Get_vnl_vector());
                affineTransform->SetOffset(pos);
                affineTransform->Modified();
                m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
                m_normalRouteModelSurface->GetGeometry()->SetOrigin(p3d2);
                m_normalRouteModelSurface->GetGeometry()->Modified();
                m_normalRouteModelSurface->Modified();


                ok = true;
                break;
            }
    case AcMOVEPOINT2: //进针 3d 2d 一样逻辑
            {
                Point2D cursor2d = posEvent->GetDisplayPosition();
                double lengthY=cursor2d[1]-m_CursorOld2d[1];
                Vector3D V3d=m_v3dModelDirection;
                V3d.Normalize();
                Point3D p3d=m_OriginOld+V3d*lengthY;
                mitk::AffineTransform3D::Pointer affineTransform = m_normalRouteModelSurface->GetGeometry()->GetIndexToWorldTransform();
                mitk::Vector3D pos;
                pos.Set_vnl_vector(p3d.Get_vnl_vector());
                affineTransform->SetOffset(pos);
                affineTransform->Modified();
                m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
                m_normalRouteModelSurface->GetGeometry()->SetOrigin(p3d);
                m_normalRouteModelSurface->GetGeometry()->Modified();
                m_normalRouteModelSurface->Modified();

                ok = true;
                break;
            }
    case AcMOVESELECTED:
            {
            if(posEvent->GetSender()->GetMapperID()==mitk::BaseRenderer::Standard3D)
                {
                        Point3D cursor = posEvent->GetWorldPosition();
                        Point3D p3d1,p3d2,p3dCam,p3dInsert;
                        mitk::Vector3D v3d1,v3d2,insertLine;Vector3D v3d;
                        double *ViewPlaneNormal= posEvent->GetSender()->GetVtkRenderer()->GetActiveCamera()->GetViewPlaneNormal();
                        v3d[0]=ViewPlaneNormal[0];
                        v3d[1]=ViewPlaneNormal[1];
                        v3d[2]=ViewPlaneNormal[2];
                        double *CameraPos=posEvent->GetSender()->GetVtkRenderer()->GetActiveCamera()->GetPosition();
                        p3dCam[0]=CameraPos[0];
                        p3dCam[1]=CameraPos[1];
                        p3dCam[2]=CameraPos[2];
                        insertLine=cursor-p3dCam;
                        insertLine=insertLine*LARGE_DOUBLE;
                        p3dInsert=p3dCam+insertLine;
                        double dbInsert[3];
                        dbInsert[0]=p3dInsert[0];
                        dbInsert[1]=p3dInsert[1];
                        dbInsert[2]=p3dInsert[2];
                        double dbOri[3];
                        dbOri[0]=m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin()[0];
                        dbOri[1]=m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin()[1];
                        dbOri[2]=m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin()[2];
                        double t;
                        double oup[3];
                        vtkPlane::IntersectWithLine(CameraPos,dbInsert,ViewPlaneNormal,dbOri,t,oup);
                        p3d1[0]=oup[0];
                        p3d1[1]=oup[1];
                        p3d1[2]=oup[2];

                        v3d1=p3d1-m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin();
                        double l1=v3d1.GetNorm();
                        if(l1>m_VecLength3d) l1=m_VecLength3d;
                        double l2=std::sqrt((m_VecLength3d*m_VecLength3d)-(l1*l1));


                        v3d2[0]=ViewPlaneNormal[0];
                        v3d2[1]=ViewPlaneNormal[1];
                        v3d2[2]=ViewPlaneNormal[2];
                        v3d2.Normalize();

                        mitk::Vector3D v3d3=p3d1-cursor;
                        v3d3.Normalize();
                        double angle=acosf(v3d3*v3d2); //acosf(v3d3*v3d2/(v3d3.GetNorm()*v3d2.GetNorm()))
                        if(angle<3.141592/2.0) l2=-l2;
                        p3d2=p3d1-v3d3*(l2);

                        Vector3D routeDir,v2,v3;
                        routeDir=p3d2-m_OriginOld;
                        routeDir.Normalize();
                        v2.Fill(0);
                        v2[0]=routeDir[2];
                        v2[2]=-routeDir[0]; // v2*routeDir==0
                        v2.Normalize();
                        vnl_vector_fixed< ScalarType, 3 > vnlDirection = vnl_cross_3d( routeDir.GetVnlVector(), v2.GetVnlVector() );
                        v3.SetVnlVector(vnlDirection);
                        v3.Normalize();
                        mitk::AffineTransform3D::Pointer affineTransform =mitk::AffineTransform3D::New();
                        mitk::Vector3D pos;
                        pos.Set_vnl_vector(m_OriginOld.Get_vnl_vector());
                        affineTransform->SetOffset(pos);
                        AffineTransform3D::MatrixType m;
                        m.GetVnlMatrix().set_column(0,routeDir.Get_vnl_vector());
                        m.GetVnlMatrix().set_column(1,v2.Get_vnl_vector());
                        m.GetVnlMatrix().set_column(2,v3.Get_vnl_vector());
                        affineTransform->SetMatrix(m);
                        affineTransform->Modified();
                        m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
                        m_normalRouteModelSurface->GetGeometry()->SetOrigin(m_OriginOld);
                        m_normalRouteModelSurface->GetGeometry()->Modified();
                        m_normalRouteModelSurface->Modified();

                        ok = true;
                        break;
                }
                else
                {
                        Point3D cursor = posEvent->GetWorldPosition();
                        Point3D needleEnd3d=m_OriginOld+m_v3dModelDirection;
                        Point3D p3d2,cursorProj,endProj;

                        Point2D cursor2d,p2dEnd,p2dOri;
                        mitk::DisplayGeometry::Pointer dg=posEvent->GetSender()->GetDisplayGeometry();
                        const mitk::Geometry2D* cwg= posEvent->GetSender()->GetCurrentWorldGeometry2D();
                        dg->Map(cursor,cursor2d);
                        dg->Map(m_OriginOld,p2dOri);
                        dg->Map(needleEnd3d,p2dEnd);
                        dg->Project(needleEnd3d,endProj);
                        dg->Project(cursor,cursorProj);

                        Vector3D needleProj=cursorProj-m_OriginOld;
                        double l1=needleProj.GetNorm();
                        if(l1>m_VecLength3d)
                                l1=m_VecLength3d;

                        double hight=std::sqrt((m_VecLength3d*m_VecLength3d)-(l1*l1));
                        Vector3D planeNormal=cwg->GetAxisVector(2);

                        planeNormal.Normalize();

                        if (cwg->SignedDistance(needleEnd3d)>=0)
                        {
                                p3d2=cursorProj+planeNormal*(hight);
                        }
                        else
                        {
                                p3d2=cursorProj-planeNormal*(hight);
                        }

                        Vector3D routeDir,v2,v3;
                        routeDir=p3d2-m_OriginOld;
                        routeDir.Normalize();
                        v2.Fill(0);
                        v2[0]=routeDir[2];
                        v2[2]=-routeDir[0]; // v2*routeDir==0
                        v2.Normalize();
                        vnl_vector_fixed< ScalarType, 3 > vnlDirection = vnl_cross_3d( routeDir.GetVnlVector(), v2.GetVnlVector() );
                        v3.SetVnlVector(vnlDirection);
                        v3.Normalize();
                        mitk::AffineTransform3D::Pointer affineTransform =mitk::AffineTransform3D::New();
                        mitk::Vector3D pos;
                        pos.Set_vnl_vector(m_OriginOld.Get_vnl_vector());
                        affineTransform->SetOffset(pos);
                        AffineTransform3D::MatrixType m;
                        m.GetVnlMatrix().set_column(0,routeDir.Get_vnl_vector());
                        m.GetVnlMatrix().set_column(1,v2.Get_vnl_vector());
                        m.GetVnlMatrix().set_column(2,v3.Get_vnl_vector());
                        affineTransform->SetMatrix(m);
                        affineTransform->Modified();
                        m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
                        m_normalRouteModelSurface->GetGeometry()->SetOrigin(m_OriginOld);
                        m_normalRouteModelSurface->GetGeometry()->Modified();
                        m_normalRouteModelSurface->Modified();
                        ok = true;
                        break;

                }
            }
    case AcMODEDESELECT:
            {

                ColorProperty::Pointer colorProperty;
                colorProperty = dynamic_cast<ColorProperty*>(m_normalRouteModel->GetProperty( "DeSelectedColor" ));
                if ( colorProperty.IsNotNull() )
                {
                    const Color& color = colorProperty->GetColor();
                    m_normalRouteModel->SetColor(color);
                }
                ok= Superclass::ExecuteAction( action, stateEvent );
                break;
            }
    case AcDESELECT:
            {
                mitk::BaseRenderer *renderer = NULL;
                vtkRenderWindow *renderWindow = NULL;
                vtkRenderWindowInteractor *renderWindowInteractor = NULL;

                renderer = posEvent->GetSender();
                if ( renderer != NULL )
                {
                  renderWindow = renderer->GetRenderWindow();
                  if ( renderWindow != NULL )
                  {
                    renderWindowInteractor = renderWindow->GetInteractor();
                  }
                }
                renderWindowInteractor->Enable();

                ColorProperty::Pointer colorProperty;
                colorProperty = dynamic_cast<ColorProperty*>(m_normalRouteModel->GetProperty( "DeSelectedColor" ));
                if ( colorProperty.IsNotNull() )
                {
                    const Color& color = colorProperty->GetColor();
                    m_normalRouteModel->SetColor(color);
                }

                ok= Superclass::ExecuteAction( action, stateEvent );
                break;
            }


    default:  return Superclass::ExecuteAction( action, stateEvent );
    }
    RenderingManager::GetInstance()->ForceImmediateUpdateAll();
    return ok;
}

void WxPointedSticksInteractor::SetRouteTipPoint(mitk::Point3D Tip)
{
    mitk::AffineTransform3D::Pointer affineTransform = m_normalRouteModelSurface->GetGeometry()->GetIndexToWorldTransform();
    mitk::Vector3D pos;
    pos.Set_vnl_vector(Tip.Get_vnl_vector());
    affineTransform->SetOffset(pos);
    affineTransform->Modified();
    m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
    m_normalRouteModelSurface->GetGeometry()->SetOrigin(Tip);
    m_normalRouteModelSurface->GetGeometry()->Modified();
    m_normalRouteModelSurface->Modified();
}

mitk::Point3D WxPointedSticksInteractor::GetRouteTipPoint()
{
    return m_normalRouteModelSurface->GetUpdatedGeometry()->GetOrigin();
}
void WxPointedSticksInteractor::SetRouteInsertPoint(mitk::Point3D InsertPoint)
{
    Point3D	Tip=GetRouteTipPoint();
    Vector3D routeDir=InsertPoint-Tip;
    routeDir.Normalize();
    Vector3D v2,v3;
    v2.Fill(0);
    v2[0]=routeDir[2];
    v2[2]=-routeDir[0]; // v2*routeDir==0
    v2.Normalize();
    vnl_vector_fixed< ScalarType, 3 > vnlDirection = vnl_cross_3d( routeDir.GetVnlVector(), v2.GetVnlVector() );
    v3.SetVnlVector(vnlDirection);
    v3.Normalize();
    mitk::AffineTransform3D::Pointer affineTransform =mitk::AffineTransform3D::New();
    mitk::Vector3D pos;
    pos.Set_vnl_vector(Tip.Get_vnl_vector());
    affineTransform->SetOffset(pos);
    AffineTransform3D::MatrixType m;
    m.GetVnlMatrix().set_column(0,routeDir.Get_vnl_vector());
    m.GetVnlMatrix().set_column(1,v2.Get_vnl_vector());
    m.GetVnlMatrix().set_column(2,v3.Get_vnl_vector());
    affineTransform->SetMatrix(m);
    affineTransform->Modified();


    m_normalRouteModelSurface->GetGeometry()->SetIndexToWorldTransform(affineTransform);
    m_normalRouteModelSurface->GetGeometry()->SetOrigin(Tip);
    m_normalRouteModelSurface->GetGeometry()->Modified();
    m_normalRouteModelSurface->Modified();
}

mitk::Point3D WxPointedSticksInteractor::GetRouteInsertPoint()
{
    float insertDistance=1;
    m_normalRouteModel->GetFloatProperty("InsertDistance", insertDistance);
    Vector3D routeDir=m_normalRouteModelSurface->GetGeometry()->GetAxisVector(0);
    routeDir.Normalize();
    return GetRouteTipPoint()+routeDir*insertDistance;
}

mitk::Point3D WxPointedSticksInteractor::GetRotation()
{
    mitk::Point3D rotation;
    typedef itk::Euler3DTransform< float > TransformType;
    TransformType::MatrixType m;
    float AngleX,AngleY,AngleZ;
    if (m_normalRouteModelSurface)
    {
        mitk::TransferMatrix(m_normalRouteModelSurface->GetUpdatedGeometry()->GetIndexToWorldTransform()->GetMatrix(), m);
    }
    AngleX = vcl_asin(m[2][1]);
    float A = vcl_cos(AngleX);
    if(vcl_fabs(A)>0.00005)
    {
        float x = m[2][2] / A;
        float y = -m[2][0] / A;
        AngleY = vcl_atan2(y,x);

        x = m[1][1] / A;
        y = -m[0][1] / A;
        AngleZ = vcl_atan2(y,x);
    }
    else
    {
        AngleZ = 0;
        float x = m[0][0];
        float y = m[1][0];
        AngleY = vcl_atan2(y,x);
    }
    rotation[0]=AngleX;
    rotation[1]=AngleY;
    rotation[2]=AngleZ;
    return rotation;
}

void WxPointedSticksInteractor::SetRotation( mitk::Point3D rotation)
{
    typedef itk::Euler3DTransform< float > TransformType;
    TransformType::Pointer eulerTransform=TransformType::New();
    eulerTransform->SetIdentity();
    eulerTransform->SetRotation(rotation[0],rotation[1],rotation[2]);
    eulerTransform->Modified();
    TransformType::MatrixType m;
    m=eulerTransform->GetMatrix();
    mitk::Surface* data=m_normalRouteModelSurface;
    if (data)
    {
        mitk::AffineTransform3D::Pointer affineTransform = data->GetGeometry()->GetIndexToWorldTransform();
        if (affineTransform.IsNull())
        {
            return;
        }
        mitk::Point3D Ori=data->GetGeometry()->GetOrigin();
        mitk::AffineTransform3D::Pointer affineTransform2=mitk::AffineTransform3D::New();
        mitk::Vector3D pos;
        pos.Set_vnl_vector(Ori.Get_vnl_vector());
        affineTransform2->SetOffset(pos);
        affineTransform2->SetMatrix(m);
        affineTransform2->Modified();
        data->GetGeometry()->SetIndexToWorldTransform(affineTransform2);
        data->GetGeometry()->SetOrigin(Ori);
        data->GetGeometry()->Modified();
        data->Modified();
    }
}
void WxPointedSticksInteractor::Roll(double angle)
{
    mitk::Surface* data=m_normalRouteModelSurface;
    if (data)
    {
        mitk::Point3D center= data->GetUpdatedGeometry()->GetOrigin();
        vtkMatrix4x4* translationMatrix = vtkMatrix4x4::New();
        translationMatrix->Identity();
        double (*transMatrix)[4] = translationMatrix->Element;
        transMatrix[0][3] = center[0];
        transMatrix[1][3] = center[1];
        transMatrix[2][3] = center[2];
        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );

        vtkTransform *vtktransform = vtkTransform::New();
        vtktransform->Identity();
        mitk::Vector3D axisOfRotation =data->GetGeometry()->GetAxisVector(2);
        vtktransform->RotateWXYZ(angle, axisOfRotation[0], axisOfRotation[1], axisOfRotation[2]);
        vtkMatrix4x4* rotationMatrix=vtktransform->GetMatrix();
        data->GetGeometry()->Compose(rotationMatrix);

        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );
        data->Modified();
        RenderingManager::GetInstance()->ForceImmediateUpdateAll();
        vtktransform->Delete();
        translationMatrix->Delete();
    }
}

void WxPointedSticksInteractor::Elevation(double angle)
{
    mitk::Surface* data=m_normalRouteModelSurface;
    if (data)
    {
        mitk::Point3D center= data->GetUpdatedGeometry()->GetOrigin();
        vtkMatrix4x4* translationMatrix = vtkMatrix4x4::New();
        translationMatrix->Identity();
        double (*transMatrix)[4] = translationMatrix->Element;
        transMatrix[0][3] = center[0];
        transMatrix[1][3] = center[1];
        transMatrix[2][3] = center[2];
        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );

        vtkTransform *vtktransform = vtkTransform::New();
        vtktransform->Identity();
        mitk::Vector3D axisOfRotation =data->GetGeometry()->GetAxisVector(0);
        vtktransform->RotateWXYZ(angle, axisOfRotation[0], axisOfRotation[1], axisOfRotation[2]);
        vtkMatrix4x4* rotationMatrix=vtktransform->GetMatrix();
        data->GetGeometry()->Compose(rotationMatrix);

        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );
        data->Modified();
        RenderingManager::GetInstance()->ForceImmediateUpdateAll();
        vtktransform->Delete();
        translationMatrix->Delete();


    }
}

void WxPointedSticksInteractor::Azimuth(double angle)
{
    mitk::Surface* data=m_normalRouteModelSurface;
    if (data)
    {
        mitk::Point3D center= data->GetUpdatedGeometry()->GetOrigin();
        vtkMatrix4x4* translationMatrix = vtkMatrix4x4::New();
        translationMatrix->Identity();
        double (*transMatrix)[4] = translationMatrix->Element;
        transMatrix[0][3] = center[0];
        transMatrix[1][3] = center[1];
        transMatrix[2][3] = center[2];
        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );

        vtkTransform *vtktransform = vtkTransform::New();
        vtktransform->Identity();
        mitk::Vector3D axisOfRotation =data->GetGeometry()->GetAxisVector(1);
        vtktransform->RotateWXYZ(angle, axisOfRotation[0], axisOfRotation[1], axisOfRotation[2]);
        vtkMatrix4x4* rotationMatrix=vtktransform->GetMatrix();
        data->GetGeometry()->Compose(rotationMatrix);

        translationMatrix->Invert();
        data->GetGeometry()->Compose( translationMatrix );
        data->Modified();
        RenderingManager::GetInstance()->ForceImmediateUpdateAll();
        vtktransform->Delete();
        translationMatrix->Delete();
    }
}
